#include <cmath>
#include <iostream>

#include "FEMModel.h"
#include "SweepDescription.h"
#include "Ray.h"
#include "Matrix.h"
#include "Setting.h"
#include "FEMDrawer.h"

#if defined(_WIN32)
#include <windows.h>
#endif

#include <gl/GL.h>
#include <gl/glu.h>

bool SweepDescription::Contains (Vector3 p)const
{
	Real coef[3];

	BarycentricCoordinates (points_, coef, p);
	return (coef[0] >= 0 && coef[1] >= 0 && coef[2] >= 0);
}

SweepDescription::SweepDescription ()
{
	count_ = 1;
}

void SweepDescription ::Precompute ()
{
	Vector3* v = points_;
	Vector3 v0 = v[0];
	Vector3 v1 = v[1];
	Vector3 v2 = v[2];

	v0 -= v2;
	v1 -= v2;

	Vector3::Cross (normal_, v0, v1);

	normal_ /= normal_.Length ();
	off_ = v2 * normal_;
	assert (normal_.Length () > 1e-8);
}

std::ostream& operator << (std::ostream &os, SweepDescription const & s)
{
	os << "sweep  { " << s.points_[0] << " "
		<< s.points_[1] << " "
		<< s.points_[2] << "} "<<endl;
	return os;
}

/*
Put some interesting values into the sweep desc.
*/
void SweepDescription::SetInterestingDefault (FiniteElementModel*fem)
{
	Vector3 still; 
	Vector3 initial_tip_direction;
	Vector3 vel ;

	still = Vector3 (0.0, -0.0, 2.0);

	Real x = GetNumberSetting ("scalpel-still-x-angle") ;
	Real y = GetNumberSetting ("scalpel-still-y-angle") ;
	cout << "SSXA SSYA " << x << " " << y <<endl;
	x *= M_PI;
	y *= M_PI;

	Matrix3 rot, rot2;
	rot.SetRotate (X_AXIS, x) ;
	rot2.SetRotate (Y_AXIS, y);

	still = rot * (rot2 * still);

	cout << still;

	Real alpha = GetNumberSetting ("scalpel-angle");
	rot.SetRotate (Z_AXIS, alpha * M_PI);
	cout << "Cut plane rotation " << alpha <<endl;

	const Real eps = 0.0023;
	Vector3 centroid;
	Real rad ;

	NodesBoundingSphere (fem, true, &centroid, &rad);

	Real st = GetNumberSetting ("scalpel-x-start");
	centroid(X_AXIS) -= st;
	still(X_AXIS) -= st;

	initial_tip_direction = centroid - still + 1.7 *rad * Vector3 (-1 + eps, eps,  eps);


	vel = centroid - (still +  initial_tip_direction) + Vector3 (0.0, eps, -0.08 - eps) ; 

	initial_tip_direction = rot * initial_tip_direction;
	vel = rot * vel;

	initial_tip_direction *= GetNumberSetting ("scalpel-length")/ initial_tip_direction.Length ();  
	vel = (GetNumberSetting ("scalpel-velocity") / vel.Length ()) * vel;

	initial_tip_direction -= vel; 

	cout << initial_tip_direction;

	points_[0] = still;
	points_[1] = still + initial_tip_direction - vel;
	points_[2] = still + initial_tip_direction + vel;

	Precompute ();
}

SweepDescription SweepDescription::NextSweep (Vector3 v) const
{
	SweepDescription next ;
	next.points_[0] = points_ [0];
	next.points_[1] = points_ [2];
	next.points_[2] = points_[2] + v;
	next.Precompute ();
	return next;
}

float scalpel_border_col [] = { 1, 0,0 ,1.0};

void SweepDescription::Visualize (FEMDrawer *draw)
{
	draw->SetColor (scalpel_border_col);
	glBegin (GL_LINE_LOOP);

	for (int i=3; i--; )
	{
		glVertex3dv (points_ [i].elts_);
	}
	glEnd ();
}